
/**
  ******************************************************************************
  * Copyright 2021 The Microbee Authors. All Rights Reserved.
  * 
  * Licensed under the Apache License, Version 2.0 (the "License");
  * you may not use this file except in compliance with the License.
  * You may obtain a copy of the License at
  * 
  * http://www.apache.org/licenses/LICENSE-2.0
  * 
  * Unless required by applicable law or agreed to in writing, software
  * distributed under the License is distributed on an "AS IS" BASIS,
  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  * See the License for the specific language governing permissions and
  * limitations under the License.
  * 
  * @file       mode_manual.c
  * @author     baiyang
  * @date       2023-7-6
  ******************************************************************************
  */

/*----------------------------------include-----------------------------------*/
#include "mode.h"
#include "fms.h"
/*-----------------------------------macro------------------------------------*/

/*----------------------------------typedef-----------------------------------*/
static enum ModeNumber mmanual_number(mode_base_const_t mode);
static const char *mmanual_name(mode_base_const_t mode);
static const char *mmanual_name4(mode_base_const_t mode);
static void mmanual_update(mode_base_t mode);
static void mmanual_run(mode_base_t mode);
/*---------------------------------prototype----------------------------------*/

/*----------------------------------variable----------------------------------*/
static struct mode_ops mode_manual_ops = {
        .run                                = mmanual_run,
        .mode_number                        = mmanual_number,
        .name                               = mmanual_name,
        .name4                              = mmanual_name4,
        .update                             = mmanual_update,
        .is_vtol_mode                       = NULL,
        .is_vtol_man_throttle               = NULL,
        .is_vtol_man_mode                   = NULL,
        .is_guided_mode                     = NULL,
        .allows_terrain_disable             = NULL,
        .does_automatic_thermal_switch      = NULL,
        .navigate                           = NULL,
        .allows_throttle_nudging            = NULL,
        .does_auto_navigation               = NULL,
        .does_auto_throttle                 = NULL,
        .mode_allows_autotuning             = NULL,
        .update_target_altitude             = NULL,
        .handle_guided_request              = NULL,
        .is_landing                         = NULL,
        .is_taking_off                      = NULL,
        ._enter                             = NULL,
        ._exit                              = NULL,
        ._pre_arm_checks                    = NULL};
/*-------------------------------------os-------------------------------------*/

/*----------------------------------function----------------------------------*/
void mmanual_ctor(struct ModeManual *mode_manual)
{
    mode_ctor(&mode_manual->mode, &mode_manual_ops);
}

// returns a unique number specific to this mode
static enum ModeNumber mmanual_number(mode_base_const_t mode)
{
    (void)mode;
    return MN_MANUAL;
}

// returns full text name
static const char *mmanual_name(mode_base_const_t mode)
{
    (void)mode;
    return "MANUAL";
}

// returns a string for this flightmode, exactly 4 bytes
static const char *mmanual_name4(mode_base_const_t mode)
{
    (void)mode;
    return "MANU";
}

static void mmanual_update(mode_base_t mode)
{
    srv_channels_set_output_scaled(k_aileron, fms_roll_in_expo(false));
    srv_channels_set_output_scaled(k_elevator, fms_pitch_in_expo(false));

    mode_output_rudder_and_steering(fms_rudder_in_expo(false));

    float throttle = fms_get_throttle_input(true);

#if HAL_QUADPLANE_ENABLED
    if (quadplane.available() && quadplane.option_is_set(QuadPlane::OPTION::IDLE_GOV_MANUAL)) {
        // for quadplanes it can be useful to run the idle governor in MANUAL mode
        // as it prevents the VTOL motors from running
        int8_t min_throttle = fms.aparm.throttle_min;

        // apply idle governor
#if AP_ICENGINE_ENABLED
        plane.g2.ice_control.update_idle_governor(min_throttle);
#endif
        throttle = MAX(throttle, min_throttle);
    }
#endif
    srv_channels_set_output_scaled(k_throttle, throttle);

    fms.nav_roll_cd = fms.ahrs->roll_sensor_cd;
    fms.nav_pitch_cd = fms.ahrs->pitch_sensor_cd;
}

static void mmanual_run(mode_base_t mode)
{
    mode_reset_controllers(mode);
}
/*------------------------------------test------------------------------------*/


